﻿using SharpDX;
using System;
using System.Collections.Generic;
using System.IO;

namespace Engine.FileFormats3D.Tracks
{
    public class WR2Track
    {
        #region Private Fields

        private short a1, a2, Turns, a4, numberOfArrows, a6, a7, a8;
        private List<Arrows> arrows;
        private int loopFlag;
        private List<Route> routes;
        private ushort wr2Flag1, wr2Flag2, u3, u4;

        #endregion Private Fields

        #region Public Constructors

        public WR2Track(List<SharpDX.Vector3> vectorsRight, List<SharpDX.Vector3> vectorsLeft, bool createIdealLine)
            : this()
        {
            float distance = GeoMath.DistanceBtw2Points(vectorsRight[0], vectorsRight[vectorsRight.Count - 1]);
            if (GeoMath.DistanceBtw2Points(vectorsRight[0], vectorsRight[vectorsRight.Count - 1]) < 200.0f)
                this.loopFlag = 1;

            this.routes.Add(new Route(vectorsRight[0], vectorsLeft[0], null));
            for (int i = 1; i < vectorsRight.Count; i++)
            {
                this.routes.Add(new Route(vectorsRight[i], vectorsLeft[i], this.routes[i - 1]));
            }

            for (int i = 0; i < routes.Count; i++)
            {
                float dx = routes[Math.Max(i - 1, 0)].Position.X - routes[Math.Min(i + 1, this.routes.Count - 1)].Position.X;
                float dz = routes[Math.Max(i - 1, 0)].Position.Z - routes[Math.Min(i + 1, this.routes.Count - 1)].Position.Z;
                float length = (float)Math.Sqrt(dx * dx + dz * dz);

                dx /= length;
                dz /= length;

                float angle;
                if (dx != 0)
                    angle = (float)Math.Atan2(-dz, dx);
                else
                    if (dz > 0)
                        angle = -(float)(Math.PI / 2.0);
                    else
                        angle = (float)(Math.PI / 2.0);

                angle -= (float)(Math.PI / 2.0);
                float differens = (vectorsLeft[i].Y + vectorsRight[i].Y) / 2;
                float halfdistance = GeoMath.DistanceBtw2Points(vectorsLeft[i], vectorsRight[i]) / 2.0f;
                Vector2 coordinates = new Vector2(halfdistance, differens - vectorsLeft[i].Y);
                coordinates.Normalize();
                float pitch = (float)Math.Atan2(coordinates.Y, coordinates.X);// coordinates.X);
                routes[i].Matrix = Matrix.RotationY(-angle) * Matrix.RotationZ(-pitch);//.RotationYawPitchRoll(-angle, 0,
            }

            for (int i = 1; i < routes.Count - 1; i++)
            {
                var difference = (routes[i - 1].Position.Y - routes[i].Position.Y);
                float angle = (float)Math.Atan2(-(difference), GeoMath.DistancePlanar(routes[i].Position, routes[i - 1].Position));
                routes[i].Matrix *= Matrix.RotationX(angle);
            }

            WR2Track.MakeIdealLine(this, createIdealLine);
        }

        public WR2Track(List<Vector3> vectorsRight, List<Vector3> vectorsLeft)
            : this(vectorsRight, vectorsLeft, true)
        {
        }

        public WR2Track()
        {
            this.routes = new List<Route>();
            this.arrows = new List<Arrows>();
        }

        #endregion Public Constructors

        #region Public Methods

        public static WR2Track FromFile(string fileName)
        {
            FileStream fs = new FileStream(fileName, FileMode.Open);
            BinaryReader br = new BinaryReader(fs);

            WR2Track track = WR2Track.FromStream(br);

            br.Close();
            fs.Close();

            return track;
        }

        public static WR2Track FromStream(BinaryReader br)
        {
            WR2Track wr2Track = new WR2Track();
            int numberOfNodes = br.ReadInt32();
            wr2Track.loopFlag = br.ReadInt32();
            wr2Track.wr2Flag1 = br.ReadUInt16();
            wr2Track.wr2Flag2 = br.ReadUInt16();
            wr2Track.u3 = br.ReadUInt16();
            wr2Track.u4 = br.ReadUInt16();

            for (int i = 0; i < numberOfNodes; i++)
            {
                wr2Track.routes.Add(Route.FromStream(br));
            }

            wr2Track.a1 = br.ReadInt16();
            wr2Track.a2 = br.ReadInt16();
            wr2Track.Turns = br.ReadInt16();
            wr2Track.a4 = br.ReadInt16();
            wr2Track.numberOfArrows = br.ReadInt16();
            wr2Track.a6 = br.ReadInt16();
            wr2Track.a7 = br.ReadInt16();
            wr2Track.a8 = br.ReadInt16();

            for (int i = 0; i < wr2Track.numberOfArrows; i++)
            {
                wr2Track.arrows.Add(Arrows.FromStream(br));
            }

            return wr2Track;
        }

        public void ToFile(string filename)
        {
            // Create folder if it doesn´t exist.
            string folderName = Path.GetDirectoryName(filename);
            if (!Directory.Exists(folderName))
                Directory.CreateDirectory(folderName);

            FileStream fs = new FileStream(filename, FileMode.Create);
            BinaryWriter bw = new BinaryWriter(fs);

            bw.Write((int)this.routes.Count);
            bw.Write((int)this.loopFlag);
            bw.Write((ushort)1);
            bw.Write((ushort)1);
            bw.Write((ushort)this.u3);
            bw.Write((ushort)this.u4);

            for (int i = 0; i < this.routes.Count; i++)
            {
                this.routes[i].ToStream(bw);
            }

            bw.Write((short)this.a1);
            bw.Write((short)1);//this.a2);
            bw.Write((short)this.Turns);
            bw.Write((short)this.a4);
            bw.Write((short)this.numberOfArrows);
            bw.Write((short)this.a6);
            bw.Write((short)this.a7);
            bw.Write((short)this.a8);

            bw.Close();
            fs.Close();
        }

        #endregion Public Methods

        #region Private Methods

        private static void MakeIdealLine(WR2Track wR2Track, bool createIdealLine)
        {
            int optimizeLevel = 1;
            int optimizeLevel2 = 1;

            if (createIdealLine)
            {
                optimizeLevel = 10;
                optimizeLevel2 = 512;
            }

            float[] ix = new float[wR2Track.routes.Count];
            float[] iy = new float[wR2Track.routes.Count];
            float[] iz = new float[wR2Track.routes.Count];
            float[] Blur = new float[wR2Track.routes.Count];
            float[] Blur2 = new float[wR2Track.routes.Count];
            float[] Ideal2 = new float[wR2Track.routes.Count];

            for (int i = 0; i < wR2Track.routes.Count; i++)
            {
                wR2Track.routes[i].Ideal = 0;
            }

            for (int i = 0; i < optimizeLevel; i++)
            {
                for (int k = 0; k < optimizeLevel2; k++)
                {
                    for (int h = 0; h < wR2Track.routes.Count; h++)
                    {
                        ix[h] = wR2Track.routes[h].Position.X + wR2Track.routes[h].Ideal * wR2Track.routes[h].Matrix.M11;
                        iy[h] = wR2Track.routes[h].Position.Y + wR2Track.routes[h].Ideal * wR2Track.routes[h].Matrix.M21;
                        iz[h] = wR2Track.routes[h].Position.Z + wR2Track.routes[h].Ideal * wR2Track.routes[h].Matrix.M31;
                    }

                    for (int h = 0; h < wR2Track.routes.Count; h++)
                    {
                        float nx1, nx2, nz1, nz2, la, lb, L, M;

                        int a = h - 1;
                        if (a < 0)
                            a = wR2Track.routes.Count - 1;

                        int b = h;
                        int c = h + 1; if (c == wR2Track.routes.Count) c = 1 + 1;

                        // Normal to AB
                        nx1 = iz[a] - iz[b]; nz1 = ix[b] - ix[a];
                        GeoMath.Normalize(ref nx1, ref nz1);

                        nx2 = iz[b] - iz[c]; nz2 = ix[c] - ix[b];
                        GeoMath.Normalize(ref nx2, ref nz2);

                        // AC half
                        L = (ix[c] - ix[a]) / 2;
                        M = (iz[c] - iz[a]) / 2;

                        la = (float)Math.Sqrt(L * L + M * M);
                        lb = (float)Math.Sqrt(Math.Pow(L + nx2 - nx1, 2) + Math.Pow(M + nz2 - nz1, 2));

                        wR2Track.routes[h].CurveRad = GeoMath.EnsureRange(-la / (la - lb) / 10, -8000, 8000); //Rad is ratio of these bases
                    }

                    for (int h = 0; h < wR2Track.routes.Count; h++)
                    {
                        int a = h - 1; if (a < 0) a = wR2Track.routes.Count - 1; //Previous node
                        int b = h;                                           //Current node
                        int c = h + 1; if (h == wR2Track.routes.Count - 1) c = 1 + 1; //Next node

                        float L = 0;
                        if (wR2Track.routes[a].CurveRad > 0) L = L + 10 / (wR2Track.routes[a].CurveRad + 5);
                        if (wR2Track.routes[a].CurveRad < 0) L = L + 10 / (wR2Track.routes[a].CurveRad - 5);

                        if (wR2Track.routes[b].CurveRad > 0) L = L + 40 / (wR2Track.routes[b].CurveRad + 5);
                        if (wR2Track.routes[b].CurveRad < 0) L = L + 40 / (wR2Track.routes[b].CurveRad - 5);

                        if (wR2Track.routes[c].CurveRad > 0) L = L + 10 / (wR2Track.routes[c].CurveRad + 5);
                        if (wR2Track.routes[c].CurveRad < 0) L = L + 10 / (wR2Track.routes[c].CurveRad - 5);

                        L = L / 3;
                        L = L * Math.Abs(L);

                        Blur[h] = wR2Track.routes[h].Ideal + L;
                        Blur2[h] = (Ideal2[a] + 2 * Ideal2[b] + Ideal2[c]) / 4;
                    }

                    float CarWidth = 22.0f;
                    for (int h = 0; h < wR2Track.routes.Count; h++)
                    {
                        float L = Blur[h] + Blur2[h];
                        float M = L - wR2Track.routes[h].Ideal;

                        wR2Track.routes[h].Ideal = L;
                        Ideal2[h] = Blur2[h];

                        if (L <= wR2Track.routes[h].Margin1 / 10 + CarWidth)
                        {
                            wR2Track.routes[h].Ideal = wR2Track.routes[h].Margin1 / 10 + CarWidth;
                            Ideal2[h] = Ideal2[h] - M;
                        }

                        if (L >= wR2Track.routes[h].Margin2 / 10 - CarWidth)
                        {
                            wR2Track.routes[h].Ideal = wR2Track.routes[h].Margin2 / 10 - CarWidth;
                            Ideal2[h] = Ideal2[h] - M;
                        }
                    }
                }
            }

            wR2Track.routes[0].Delta2 = 0;
            for (int h = 1; h < wR2Track.routes.Count; h++)
            {
                float L = (float)Math.Sqrt(Math.Pow(ix[h] - ix[h - 1], 2) + Math.Pow(iy[h] - iy[h - 1], 2) + Math.Pow(iz[h] - iz[h - 1], 2));
                wR2Track.routes[h].Delta2 = wR2Track.routes[h - 1].Delta2 + L;
            }
        }

        #endregion Private Methods
    }
}